AdmittanceController 源码精读与全流程走查

读控制器源码有两种顺序：直接从 on_init 一路读下去，或者先把系统在 mock 硬件上跑起来、对照看得见的现象再读代码。后者的收益大得多——本文就是一次「先跑通、再读码」的完整走查实录，对象是 ros2_controllers（Humble，commit 2105376）的 AdmittanceController：先用 mock 硬件观察六个现场并核对验收数字，再锚定到源码的生命周期、参数系统与关键机制，最后汇总走查中暴露的排坑记录。适合正在读 ros2_control 源码、或要在它上面做控制器二次开发的读者。架构角色与 chainable 机制的背景见本站《ros2_control 控制器架构与 chainable 机制》，本文不重复。

先跑通：六个现场与验收数字

mock 硬件下给末端挂一个恒力（initial_value 写死的力传感器读数），观察到的验收数字全部与理论对得上：

• 偏移模长 0.10000 m = 10 N ÷ 100 N/m（F/K）
• y 分量精确为 0：初始构型 j1=j4=j6=0，运动锁在 base 的 xz 平面
• rotation 恒等（3.6e-16 浮点噪声）：selected_axes 后三项 false，旋转轴被掩码
• 方向 = 传感器 −z 轴在 base 帧的投影：臂精确沿受力方向让位
• status 100.0 Hz（抖动 1 ms，普通调度、非 FIFO RT）

数字「过分干净」本身是有信息的：它说明系统退化成了可以手算的形态（恒力对纯弹簧），读码时就能预期每个项各自贡献什么。

再读码：生命周期四时刻（admittance_controller.cpp）

时刻	行号	内容
on_init	:35-60	ParamListener 构造（:40）+ 构造 rule（:41，纯 C++ 类、unique_ptr 成员，构造即参数快照 hpp:103-107）；解析 robot_description（:62-74）；预分配五个轨迹点容器（:51-60，实时路径零分配从第一行守起）
on_configure	:182-332	四件事：参数校验（command_joints 回退 :183-190）；接口声明与导出 reference interfaces（chainable 本体）；话题订阅回调只写 RealtimeBuffer（:305-312）；pluginlib 加载 KDL（:325-329，rule 侧 impl:44-79）
on_activate	:334-400	接口按 joints 排序；reference_ = joint_state_（:394-397）——初始参考 = 激活时刻实际位姿，漂移起点
update_and_write_commands	:437	四拍：读参考 → 读硬件（NaN 用 last_commanded_ 后备 :551-562）→ rule 数学 → 写回。力传感器 NaN 清零（:564-572）

参数系统：generate_parameter_library（GPL）

定义 yaml（src/admittance_controller_parameters.yaml，160 行）→ CMake 一行 generate_parameter_library(...)（:37-38）→ build 期生成 Params 结构体 + ParamListener（声明/类型校验/get_params 快照/is_old 热更新）。与 IDL→typesupport 同型的 codegen。要点：

• joints 无 default_value 且 read_only：外部必须提供（load 阶段报 expected [string_array] got [not set] 的结构根源），激活后改不动
• chainable_command_interfaces 默认 ["position","velocity"]：yaml 里没写它、spawner 日志照样打两条 chainable 行
• stiffness：fixed_size 6 + element_bounds [0, 1e8]；damping_ratio 的官方 description 自带 ζ = D/(2√(MS))
• 在线可调性由 read_only 决定；生效路径 is_old() → get_params()（impl:121-123）

概念澄清

• 帧 vs 位姿：同一 Eigen::Isometry3d 的双重读法——换算器（p_base = T·p_sensor，:187 的旋转）与描述符（X 的计算 :261-265）。帧的 6 DOF 决定控制器里一切向量都是 6 维。
• kinematics：FK 关节角→位姿唯一解（每周期 7 次）；IK 反问题可无解/多解，MoveL 逐点 IK 即它；Jacobian 是瞬时汇率 ẋ = J(q)·q̇，随构型变，奇异处降秩 → 微分 IK 用阻尼伪逆（alpha）。
• claim：ResourceManager 是全局接口注册表（硬件接口 + 各控制器导出的 reference 接口同表）。command 接口独占单写者（resource_manager.hpp:212 原文 "Any command interface can only be claimed by a single instance"），state 接口任意多读者；Loaned*Interface RAII，析构归还。单写者原则由 RM 强制，不是约定——这是链式架构（JTC 写 admittance 参考口、admittance 写硬件）能共存的机制保证。
• command_joints：输出路由前缀，不是别名。拼接规则 条目 + "/" + 接口类型（cpp:104-107）——填 joint1 claim 硬件接口；填 jtc/joint1 claim JTC 的 reference 接口，链方向反转（admittance 当链头）。默认空回退 joints（日志有回退提示）。
• RealtimeBuffer：API 后缀声明调用者住哪侧（readFromRT = 实时线程发起的读，writeFromNonRT = executor 线程发起的写）。双缓冲 + 原子指针交换，两侧 wait-free；单槽 newest-wins（旧设定点静默丢弃，对 setpoint 流正确）；模板参数是 shared_ptr，跨界只拷 8 字节指针。
• 重力补偿 vs 偏置：三层分解——实测 = 接触力 + 重力静读数（可建模）+ 零点漂移（不可建模）。补偿是控制器层在 world 帧做的（impl:336-337）：力行只减 z（world 里重量恒为 [0,0,−w]），力矩减 r×w。姿态不变性成立的原因：姿态影响全部住在 :187 每拍从当前关节角重算的旋转里，不在减法行。零漂靠开工前传感器清零，这套栈里无标准件。
• 低通滤波（系数 ft_sensor.filter_coefficient，上游默认 0.05，此处按 α=0.1 估算）：压传感器电气噪声、机械振铃、100 Hz 采样的混叠；代价 τ ≈ dt(1−α)/α ≈ 0.09 s（α=0.1、dt=10 ms）。与 M-D-K 瞬态（ω = √(K/M) = 10 rad/s）合成走查时看到的「几秒才稳定」。

排坑实录

现象	根因	修法
load 报 expected [string_array] got [not set]	控制器参数写成嵌套在 controller_manager 名下的 ros__parameters——全 ros2_control 源码无任何搬运代码，rclcpp 只按节点同名顶层 key 分发 params-file	参数树放顶层 admittance_controller: key，与 CM 平级
Expected 6 'position' command interfaces, got 1	URDF 的 ros2_control 块只示范了 joint1 的接口块，其余五个关节没登记	六个关节的 command/state 接口块全部显式写出
ros2 control spawn_controller 命令不存在	ros2controlcli 在 humble/iron/rolling 各分支均无此动词，易与 spawner 可执行文件混淆	用 ros2 run controller_manager spawner/unspawner；且 Humble 的 load 只做 on_init，终态 [unconfigured]，configure 是独立服务（cm.cpp:794）
激活正常但 admittance_position 恒 0	裸 <state_interface name="torque.x"/> 无 initial_value → GenericSystem 默认初始化为 NaN → NaN 降级（:564-572）因任一分量 NaN 清零整只 wrench，−10 N 恒力陪葬	FT 六个接口全部给 initial_value（用不到的写 0.0）
RViz 看不到模型（三因叠加）	① 纯空 link 无 <visual> 几何，RViz 无物可画；② Fixed Frame 默认 map，TF 树无 map → 一串 No transform；③ /robot_description 是 transient_local（latched），RViz 揣着坏时期消息不再重收	给 link 加杆件几何；Fixed Frame 改 base_link；重启 RViz 重建订阅
[Deprecated] Passing the robot description parameter directly...	参数喂 URDF 方式已弃用，但 CM 警告后照常初始化（cm.cpp:293-301）	无需处理，Humble 两种方式都支持

要点

• 先跑通再读码的收益：六个现场与 5 处文档勘误全部由实操暴露，比单纯读代码更能验证理解。
• mock 硬件两条铁律：接口要么给 initial_value 要么默认 NaN；NaN 降级是静默的，出了问题要从现象自己看出来。
• ros2_control 参数按节点名顶层分发；CM 的名册只登记 名字→type。
• 重力补偿的姿态不变性靠「先旋 world 再减」，旋转每拍从当前 FK 重算——减法行本身姿态无关。
• 真机部署的三个前置：fixed_world_frame 对准真实重力、CoG 参数标定、传感器开工清零。

导纳定律内部那六个坐标位姿的来历与消费关系，展开见本站《AdmittanceTransforms 的六个位姿：导纳定律的坐标桥梁》；在 AdmittanceRule 上注入期望力的完整二次开发与排坑，见本站《AdmittanceController 恒力控制二次开发踩坑记》。